//**********************************Motor.c***********************************
//Trevor Wood, Matt Villarrubia
//EE345M, Spring 2011
//*****************************************************************************
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "driverlib/pwm.h"
#include "os.h"
#include "motor.h"
#include "OS.h"
#include "FIFO.h"


//********************************Motor_Init***********************************
//	Configures the PWMs used for the motors.
//! \param None
//! \return None
//*****************************************************************************
void Motor_Init(){
	// PWM initialization
  	SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM);
  	
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);

  	GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, GPIO_PIN_0 | GPIO_PIN_1 );//PWM 2 and 3
	GPIOPinTypeGPIOOutput(GPIO_PORTE_BASE, GPIO_PIN_0 | GPIO_PIN_1 );//PWM 4 and 5
  	
	GPIOPinConfigure(GPIO_PB0_PWM2);
  	GPIOPinConfigure(GPIO_PB1_PWM3);
	GPIOPinConfigure(GPIO_PE0_PWM4);
  	GPIOPinConfigure(GPIO_PE1_PWM5);

	GPIOPinTypePWM(GPIO_PORTB_BASE, GPIO_PIN_0 | GPIO_PIN_1);
	GPIOPinTypePWM(GPIO_PORTE_BASE, GPIO_PIN_0 | GPIO_PIN_1);
  	
	SysCtlPWMClockSet( SYSCTL_PWMDIV_32 );
  
  	PWMGenConfigure(PWM_BASE, PWM_GEN_1, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC | PWM_GEN_MODE_DBG_RUN);
	PWMGenConfigure(PWM_BASE, PWM_GEN_2, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC | PWM_GEN_MODE_DBG_RUN);
  	
	PWMGenPeriodSet(PWM_BASE, PWM_GEN_1, 1000);   //50mhz clock--22ms period
	PWMGenPeriodSet(PWM_BASE, PWM_GEN_2, 1000);   //50mhz clock--22ms period

  	PWMOutputInvert(PWM_BASE, (PWM_OUT_2_BIT | PWM_OUT_3_BIT | PWM_OUT_4_BIT | PWM_OUT_5_BIT), true);
  
  	PWMPulseWidthSet(PWM_BASE, PWM_OUT_2, 500 );
  	PWMPulseWidthSet(PWM_BASE, PWM_OUT_3, 500 );
	PWMPulseWidthSet(PWM_BASE, PWM_OUT_4, 500 );
  	PWMPulseWidthSet(PWM_BASE, PWM_OUT_5, 500 );
 
  
  	PWMGenEnable(PWM_BASE, PWM_GEN_1);
	PWMGenEnable(PWM_BASE, PWM_GEN_2);

  	PWMOutputState(PWM_BASE, (PWM_OUT_2_BIT | PWM_OUT_3_BIT | PWM_OUT_4_BIT | PWM_OUT_5_BIT), true);//Enable the PWM ports

}

static int constrain(int value, int min, int max){
  if(value < min)
    return min;
  if(value > max)
    return max;
  return value;
}

#define KP_LEFT_FOLLOW	3.4
#define KI_LEFT_FOLLOW	.2
#define G_DT 0.05

unsigned short LeftFollow = 1;
unsigned int CommandDistanceLeft = 0x160;
int MotorError = 0;
extern _tFIFO DistanceLeft;
extern _tFIFO DistanceFront;
extern _tFIFO DistanceRight;

void MotorTask(OS_TCB* this){
	unsigned long irLeft, irFront, irRight;
	signed long err_distance_left; 
	signed long distance_left_I = 0;
	unsigned long data; 
	int control = 0;
	int i;
	
	//initialize motor controller
	Motor_Init();
	OS_Sleep(this,100);
	while(1){
		//sample ir sensors
		i=0; 
		irLeft = 0;
		while(0 == FIFO_GetSingle(DistanceLeft, &data)){
			i++;
			irLeft += data;
		}
		if(i == 0){
			MotorError++;
			
		}
		else{
			irLeft = irLeft / i;
			// LEFT FOLLOW PI CONTROLLER
			if( LeftFollow == 1 ){
			    // P term
			    err_distance_left = CommandDistanceLeft - irLeft; 
			    err_distance_left = constrain(err_distance_left*KP_LEFT_FOLLOW,-50,50);  // to limit max roll command...
			    // I term
			    distance_left_I += err_distance_left* G_DT * KI_LEFT_FOLLOW;
			    distance_left_I = constrain(distance_left_I,-10,10);
			    // PI control
			    control = constrain(err_distance_left + distance_left_I,-50,50);
			}	// neg means needs to turn left, positive->right
		}
		
		

		//front
		i=0;
		irRight = 0;
		while(0 == FIFO_GetSingle(DistanceRight, &data)){
			i++;
			irRight += data;
		}
		if(i > 0){
			MotorError++;
		}
		else{
			irRight = irRight / i;
		}


//front
		i=0;
		irFront = 0;
		while(0 == FIFO_GetSingle(DistanceFront, &data)){
			i++;
			irFront += data;
		}
		if(i > 0){
			MotorError++;
		}
		else{
			irFront = irFront / i;
		}
		
		//check if there is a wall in front of us
		if(irFront > 900){ //turn
			Motor_Set(MOTOR_LEFT,  -800);
			Motor_Set(MOTOR_RIGHT,  800);
			OS_Sleep(this,30); 
		}
		else if(irRight > 900){
			Motor_Set(MOTOR_LEFT,  600);
			Motor_Set(MOTOR_RIGHT, -600);
			//OS_Sleep(this,10);
		}
		else{
		 	if(control>0){  //slows down right
				Motor_Set(MOTOR_LEFT, 999);
				Motor_Set(MOTOR_RIGHT,  999 - control * 12);
			}
			else{		 	//negative control slows down left
				Motor_Set(MOTOR_RIGHT, 999);
				Motor_Set(MOTOR_LEFT,  999 + control * 12);
			}
		}
						//Motor_Set(MOTOR_RIGHT, 800);
				//Motor_Set(MOTOR_LEFT,  -800);


		OS_Sleep(this,50);   //run at 20hz
	}


}

//******************************Motor_Drive************************************
//	This is the main function that drives the robot
//! \param Pointer to this thread
//! \return None
//*****************************************************************************
void Motor_Drive(OS_TCB* this)
{   
	int i; 
  	/*while(1){

  		PWMPulseWidthSet(PWM_BASE, PWM_OUT_2, 1 );
  		PWMPulseWidthSet(PWM_BASE, PWM_OUT_3, 999 );

		PWMPulseWidthSet(PWM_BASE, PWM_OUT_4, 1 );
  		PWMPulseWidthSet(PWM_BASE, PWM_OUT_5, 999 );
  	
		OS_Sleep(this,3000);
  		
		PWMPulseWidthSet(PWM_BASE, PWM_OUT_2, 999 );
  		PWMPulseWidthSet(PWM_BASE, PWM_OUT_3, 1 );

		PWMPulseWidthSet(PWM_BASE, PWM_OUT_4, 999 );
  		PWMPulseWidthSet(PWM_BASE, PWM_OUT_5, 1 );
  	
		OS_Sleep(this,3000);
  	
		PWMPulseWidthSet(PWM_BASE, PWM_OUT_2, 1 );
  		PWMPulseWidthSet(PWM_BASE, PWM_OUT_3, 1 );

		PWMPulseWidthSet(PWM_BASE, PWM_OUT_4, 1 );
  		PWMPulseWidthSet(PWM_BASE, PWM_OUT_5, 1 );

		OS_Sleep(this,3000);

  	}*/
	while(1){
		for(i=1; i<5; i++){
			Motor_Set(MOTOR_LEFT,i*200);
			Motor_Set(MOTOR_RIGHT,i*200);
			OS_Sleep(this,1000);
		}

		for(i=1; i<5; i++){
			Motor_Set(MOTOR_LEFT,i*-200);
			Motor_Set(MOTOR_RIGHT,i*-200);
			OS_Sleep(this,1000);
		}
		

		Motor_Set(MOTOR_LEFT,1);
		Motor_Set(MOTOR_RIGHT,1);
		
		OS_Sleep(this,3000);
		
			


	}
}

static void set_speed(int speed, unsigned long FIRST_PWM, unsigned long SECOND_PWM){
	if(speed < 0){
		PWMPulseWidthSet(PWM_BASE, FIRST_PWM, 1 );
  		PWMPulseWidthSet(PWM_BASE, SECOND_PWM, (-1*speed));
	}else{
		PWMPulseWidthSet(PWM_BASE, FIRST_PWM, speed);
  		PWMPulseWidthSet(PWM_BASE, SECOND_PWM, 1);
	}
}

void Motor_Set(unsigned int MOTOR_BASE, int speed){
	if(speed >999){
	 	speed = 999;
	}
	if(speed < -999){
		speed = -999;
	}
	if(speed == 0){
		speed = 1;
	}

	if(MOTOR_BASE){
		set_speed(speed,PWM_OUT_2,PWM_OUT_3);
	}else{
		set_speed(speed,PWM_OUT_4,PWM_OUT_5);
	}
}




